#include "element.h"
#include "control.h"
#include "imu.h"
#include "encode.h"
#include "adc128s.h"
#include "bmi088.h"
#include "mymath.h"
#include "vl53l0x.h"
#include "led.h"
#include "buzz.h"
#include "EC11.h"
#include "dma.h"
#include "zf_eeprom.h"
#include "zf_delay.h"
#include <string.h>

Elem_type Elem;

// 爆内存了，在edata里这个用不了
Response_type xdata Response[6][6] = {
	// Block
	{{GO_STRAIGHT, 0, 0, 60, 3000},
	 {TURN_LEFT, 1, 70, 90, 8000},
	 {TURN_RIGHT, 2, -70, 90, 7600},
	 {TURN_RIGHT, 3, -50, 90, 7200},
	 {TURN_LEFT, 4, 70, 90, 400},
	 {QUIT, 0, 0, 0, 0}},
	// Round_In
	{{GO_STRAIGHT, 0, 14, 150, 4600},
	 {TURN_RIGHT, 1, -44, 150, 4000},
	 {QUIT, 0, 0, 0, 0}},
	// Round_Out
	{{GO_STRAIGHT, 0, 0, 150, 9000},
	 {QUIT, 0, 0, 0, 0}},
	// Into_The_Garage_And_Stop
	{{GO_STRAIGHT, 0, 0, 10, 2000},
	 {GO_STRAIGHT, 1, 0, -80, -1},
	 {TURN_LEFT, 2, 90, -80, -4600},
	 {QUIT, 0, 0, 0, 0}},
	// Turn_Right_Into_The_Garage
	{{TURN_RIGHT, 0, -106, 70, 10000},
	 {QUIT, 0, 0, 0, 0}},
	// Start_And_Out_Of_The_Garage
	{{GO_STRAIGHT, 0, 0, 150, 1000},
	 {TURN_RIGHT, 1, -50, 150, 2000},
	 {QUIT, 0, 0, 0, 0}}};

typedef struct
{
	uint16 BlockDist;
	uint16 CcCopy;
	int16 CarVelocity;
	bool LaserSW;
} CarSettings_type;

CarSettings_type xdata CarSettings;

void Elem_Init(void)
{
	uint8 i;

	Elem.OutDirect = RightDirect;
	Elem.BlockDist = 85;
	Elem.LaserSW = DISABLE;

	Elem_Reset(OFF);
	// 读取e2prom
	iap_read_bytes(ROUNDIN_POINT_ADDR, E2PROMBuffer, 28);
	// 把缓存的值读取出来
	for (i = 0; i < 28; i += 4)
	{
		Elem.RoundInMax[i / 4] = (E2PROMBuffer[i] << 8) + E2PROMBuffer[i + 1];
		Elem.RoundInMin[i / 4] = (E2PROMBuffer[i + 2] << 8) + E2PROMBuffer[i + 3];
	}
	iap_read_bytes(ROUNDOUT_POINT_ADDR, E2PROMBuffer, 28);
	// 把缓存的值读取出来
	for (i = 0; i < 28; i += 4)
	{
		Elem.RoundOutMax[i / 4] = (E2PROMBuffer[i] << 8) + E2PROMBuffer[i + 1];
		Elem.RoundOutMin[i / 4] = (E2PROMBuffer[i + 2] << 8) + E2PROMBuffer[i + 3];
	}
}

void Elem_E2PROM(void)
{
	if (Elem.FlashRead == ENABLE)
	{
		LED_SetColor(GREEN);
		delay_ms(60);
		Elem.FlashRead = FALSE;
		// 读取e2prom数据出来跑赛道
		iap_read_bytes(ELEM_RESPONSE_ADDR, E2PROMBuffer, sizeof(Response));
		// 复制数据
		memcpy(Response, E2PROMBuffer, sizeof(Response));

		// 读取e2prom数据出来跑赛道
		iap_read_bytes(CTRL_SETTINGS_ADDR, E2PROMBuffer, sizeof(CarSettings));
		// 复制数据
		memcpy(&CarSettings, E2PROMBuffer, sizeof(CarSettings));

		Elem.BlockDist = CarSettings.BlockDist;
		Ctrl.CcCopy = CarSettings.CcCopy;
		Ctrl.Cc = Ctrl.CcCopy / 1000.0f;
		CarVelocity = CarSettings.CarVelocity;
		Elem.LaserSW = CarSettings.LaserSW;
	}
	if (Elem.FlashWrite == ENABLE)
	{
		LED_SetColor(GREEN);
		delay_ms(60);
		Elem.FlashWrite = FALSE;
		// 擦除数据
		iap_erase_page(ELEM_RESPONSE_ADDR + 511);
		// 复制数据
		memcpy(E2PROMBuffer, Response, sizeof(Response));
		// 保存赛道数据到e2prom
		iap_write_bytes(ELEM_RESPONSE_ADDR, E2PROMBuffer, sizeof(Response));

		CarSettings.BlockDist = Elem.BlockDist;
		Ctrl.CcCopy = Ctrl.Cc * 1000;
		CarSettings.CcCopy = Ctrl.CcCopy;
		CarSettings.CarVelocity = CarVelocity;
		CarSettings.LaserSW = Elem.LaserSW;

		// 擦除数据
		iap_erase_page(CTRL_SETTINGS_ADDR + 511);
		// 复制数据
		memcpy(E2PROMBuffer, &CarSettings, sizeof(CarSettings));
		// 保存赛道数据到e2prom
		iap_write_bytes(CTRL_SETTINGS_ADDR, E2PROMBuffer, sizeof(CarSettings));
	}
}

void Elem_Identify(void)
{
	// 坡道判断
	if (ADC.Raw[4] > ADC.Std[3] * 1.2f)
	{
		Elem.StartTime = Counter.Tim4;
		Elem.SubRoad = Rampway;
	}
	if (Elem.SubRoad == Rampway)
	{
		Ctrl.Velocity = 0.6f * CarVelocity;
		// 坡道持续时间
		if (Counter.Tim4 - Elem.StartTime > 500)
		{
			Elem.SubRoad = No_Elem;
		}
	}
	else if (Elem.SubRoad == OpenJunction)
	{
		Ctrl.Velocity = 0.9f * CarVelocity;
		// 断路持续时间判断
		if (P03 == OFF)
		{
			Elem.SubRoad = No_Elem;
			// 小于十毫秒则激光入库
			if (Counter.Tim4 - Elem.StartTime < 100)
			{
				Elem_Reset(ON);
				Elem.Road = Turn_Right_Into_The_Garage;
				Ctrl.Velocity = Response[Turn_Right_Into_The_Garage][0].Speed;
				Elem.State = Response[Turn_Right_Into_The_Garage][0].State;
				IMU_SetYaw(Response[Turn_Right_Into_The_Garage][0].Angle);
			}
		}
	}
	else
	{
		Ctrl.Velocity = CarVelocity;
	}
	if (Counter.Main > 200 && Elem.SubRoad == No_Elem)
	{
		// 圆环识别函数
		Round_Identify();
		// 有障碍识别函数
		Block_Identify();
	}
}

void Elem_Response(void)
{
	uint8 i;
	if (Elem.Road <= 5)
	{
		for (i = 0; Response[Elem.Road][i].State != QUIT; i++)
		{
			if (Elem.State == Response[Elem.Road][i].State && Elem.Step == Response[Elem.Road][i].Step)
			{
				if (Response[Elem.Road][i].Displace >= 0)
				{
					if (Enco.Displace > Response[Elem.Road][i].Displace)
					{
						if (Response[Elem.Road][i + 1].State == QUIT)
						{
							Ctrl.Velocity = CarVelocity;
							IMU_SetYaw(0);
							if (Elem.Road == Into_The_Garage_And_Stop || Elem.Road == Turn_Right_Into_The_Garage)
							{
								Ctrl.Velocity = 0;
								Ctrl.Car = DISABLE;
								Ctrl.GarageInto = ENABLE;
							}
							Elem_Reset(OFF);
						}
						else
						{
							Enco.Displace = 0;
							Ctrl.Velocity = Response[Elem.Road][i + 1].Speed;
							Elem.State = Response[Elem.Road][i + 1].State;
							Elem.Step++;
							IMU_SetYaw(Response[Elem.Road][i + 1].Angle);
						}
					}
				}
				else
				{
					if (Enco.Displace < Response[Elem.Road][i].Displace)
					{
						if (Response[Elem.Road][i + 1].State == QUIT)
						{
							Ctrl.Velocity = CarVelocity;
							IMU_SetYaw(0);
							if (Elem.Road == Into_The_Garage_And_Stop || Elem.Road == Turn_Right_Into_The_Garage)
							{
								Buzz_ON();
								Ctrl.Velocity = 0;
								Ctrl.Car = DISABLE;
								Ctrl.GarageInto = ENABLE;
							}
							Elem_Reset(OFF);
						}
						else
						{
							Enco.Displace = 0;
							Ctrl.Velocity = Response[Elem.Road][i + 1].Speed;
							Elem.State = Response[Elem.Road][i + 1].State;
							Elem.Step++;
							IMU_SetYaw(Response[Elem.Road][i + 1].Angle);
						}
					}
				}
				break;
			}
		}
	}
}

void Elem_Reset(uint8 _buzz)
{
	Elem.Road = No_Elem;
	Counter.Main = 0;
	Enco.Displace = 0;
	Elem.State = QUIT;
	Elem.Step = 0;

	if (_buzz == ON)
	{
		Buzz_ON();
	}
	else
	{
		Buzz_OFF();
	}
}

void RoundIn_ScanPoint(void)
{
	uint8 i;
	uint16 j;

	// 初始化最大最小值
	for (i = 0; i < 7; i++)
	{
		Elem.RoundInMax[i] = 0;
		Elem.RoundInMin[i] = 4095;
	}
	// 采集两千五百次，五秒完成
	for (j = 0; j < 2500; j++)
	{
		delay_ms(2);
		// 采集原始数据
		ADC_GetData();
		// 取最大或最小
		for (i = 0; i < 7; i++)
		{
			if (ADC.Raw[i + 1] > Elem.RoundInMax[i])
			{
				Elem.RoundInMax[i] = ADC.Raw[i + 1];
			}
			if (ADC.Raw[i + 1] < Elem.RoundInMin[i])
			{
				Elem.RoundInMin[i] = ADC.Raw[i + 1];
			}
		}
	}
	// 最大最小转换成uint8，以便保存
	for (i = 0; i < 28; i += 4)
	{
		E2PROMBuffer[i] = (uint8)(Elem.RoundInMax[i / 4] >> 8);
		E2PROMBuffer[i + 1] = (uint8)Elem.RoundInMax[i / 4];
		E2PROMBuffer[i + 2] = (uint8)(Elem.RoundInMin[i / 4] >> 8);
		E2PROMBuffer[i + 3] = (uint8)Elem.RoundInMin[i / 4];
	}
	// 擦除原有的数据
	iap_erase_page(ROUNDIN_POINT_ADDR + 511);
	// 保存赛道数据到e2prom
	iap_write_bytes(ROUNDIN_POINT_ADDR, E2PROMBuffer, 28);
}

void RoundOut_ScanPoint()
{
	uint8 i;
	uint16 j;

	// 初始化最大最小值
	for (i = 0; i < 7; i++)
	{
		Elem.RoundOutMax[i] = 0;
		Elem.RoundOutMin[i] = 4095;
	}
	// 采集两千五百次，五秒完成
	for (j = 0; j < 2500; j++)
	{
		delay_ms(2);
		// 采集原始数据
		ADC_GetData();
		// 取最大或最小
		for (i = 0; i < 7; i++)
		{
			if (ADC.Raw[i + 1] > Elem.RoundOutMax[i])
			{
				Elem.RoundOutMax[i] = ADC.Raw[i + 1];
			}
			if (ADC.Raw[i + 1] < Elem.RoundOutMin[i])
			{
				Elem.RoundOutMin[i] = ADC.Raw[i + 1];
			}
		}
	}
	// 最大最小转换成uint8，以便保存
	for (i = 0; i < 28; i += 4)
	{
		E2PROMBuffer[i] = (uint8)(Elem.RoundOutMax[i / 4] >> 8);
		E2PROMBuffer[i + 1] = (uint8)Elem.RoundOutMax[i / 4];
		E2PROMBuffer[i + 2] = (uint8)(Elem.RoundOutMin[i / 4] >> 8);
		E2PROMBuffer[i + 3] = (uint8)Elem.RoundOutMin[i / 4];
	}
	// 擦除原有的数据
	iap_erase_page(ROUNDOUT_POINT_ADDR + 511);
	// 保存赛道数据到e2prom
	iap_write_bytes(ROUNDOUT_POINT_ADDR, E2PROMBuffer, 28);
}

/**
 * @brief     圆环识别函数
 * @brief     函数说明：当检测到进圆环标记时，启用偏航PID进入圆环，当遇到在环中标记时启用电磁循迹PID
 * @retval    none
 */
void Round_Identify(void)
{
	static uint8 RoundInto = 0;

	if (ADC.Raw[7] >= Elem.RoundInMin[6] && ADC.Raw[7] <= Elem.RoundInMax[6] &&
		ADC.Raw[1] >= Elem.RoundInMin[0] && ADC.Raw[1] <= Elem.RoundInMax[0] &&
		ADC.Raw[6] >= Elem.RoundInMin[5] && ADC.Raw[6] <= Elem.RoundInMax[5] &&
		ADC.Raw[2] >= Elem.RoundInMin[1] && ADC.Raw[2] <= Elem.RoundInMax[1] &&
		ADC.Raw[5] >= Elem.RoundInMin[4] && ADC.Raw[5] <= Elem.RoundInMax[4] &&
		ADC.Raw[3] >= Elem.RoundInMin[2] && ADC.Raw[3] <= Elem.RoundInMax[2] &&
		ADC.Raw[4] >= Elem.RoundInMin[3] && ADC.Raw[4] <= Elem.RoundInMax[3] &&
		RoundInto == 0)
	{
		RoundInto = 1;
		Elem_Reset(ON);
		Elem.Road = Round_In;
		Ctrl.Velocity = Response[Round_In][0].Speed;
		Elem.State = Response[Round_In][0].State;
		IMU_SetYaw(Response[Round_In][0].Angle);
	}
	else if (ADC.Raw[7] >= Elem.RoundOutMin[6] && ADC.Raw[7] <= Elem.RoundOutMax[6] &&
			 ADC.Raw[1] >= Elem.RoundOutMin[0] && ADC.Raw[1] <= Elem.RoundOutMax[0] &&
			 ADC.Raw[6] >= Elem.RoundOutMin[5] && ADC.Raw[6] <= Elem.RoundOutMax[5] &&
			 ADC.Raw[2] >= Elem.RoundOutMin[1] && ADC.Raw[2] <= Elem.RoundOutMax[1] &&
			 ADC.Raw[5] >= Elem.RoundOutMin[4] && ADC.Raw[5] <= Elem.RoundOutMax[4] &&
			 ADC.Raw[3] >= Elem.RoundOutMin[2] && ADC.Raw[3] <= Elem.RoundOutMax[2] &&
			 ADC.Raw[4] >= Elem.RoundOutMin[3] && ADC.Raw[4] <= Elem.RoundOutMax[3] &&
			 RoundInto == 1)
	{
		RoundInto = 0;
		Elem_Reset(ON);
		Elem.Road = Round_Out;
		Ctrl.Velocity = Response[Round_Out][0].Speed;
		Elem.State = Response[Round_Out][0].State;
		IMU_SetYaw(Response[Round_Out][0].Angle);
	}
}

/**
 * @brief          路障判定和动作执行函数
 * @retval         none
 */
void Block_Identify(void)
{
	// 获取夏普红外20-150cm传感器的距离值
	//-12.6793  116.9868 -429.6827  793.8764 -768.6862  360.7923
	// ADC.InfVol=-12.6793f*ADC.InfVol*ADC.InfVol*ADC.InfVol*ADC.InfVol*ADC.InfVol
	// +116.9868f*ADC.InfVol*ADC.InfVol*ADC.InfVol*ADC.InfVol
	// -429.6827f*ADC.InfVol*ADC.InfVol*ADC.InfVol
	// +793.8764f*ADC.InfVol*ADC.InfVol
	// -768.6862f*ADC.InfVol
	// +360.7923f;
	// 22.3288 -166.4483  457.5147 -573.6003  319.9617
	// 实测发现四次多项式已经满足条件，再根据实际测量的结果补偿了8cm
	// 实测20cm不可用，30-70cm非常准确，80-100非常偏离但都大于本身比如实际80得85-95
	// 要拐弯的点应该在50cm左右，足够了
	// 放control了Elem.Distance = 22.3288f * ADC.InfVol * ADC.InfVol * ADC.InfVol * ADC.InfVol - 166.4483f * ADC.InfVol * ADC.InfVol * ADC.InfVol + 457.5147f * ADC.InfVol * ADC.InfVol - 573.6003f * ADC.InfVol + 327;
	// 夏普红外10-80cm传感器五次多项式系数-6.3681 62.1627 - 236.5428 444.1693 - 427.7095 192.0019

	if (M_ABS(Elem.Distance) <= Elem.BlockDist) // 这是夏普红外的，阳光不直射应该没问题
	// if (ADC.InfVol < 0.4f)这是激光传感器的，但因为广告牌反射的原因肯定用不了
	{
		Counter.Main = 0;
		if (ADC.Raw[4] <= (ADC.Max[3] * 1.06f))
		{
			Elem_Reset(ON);
			Elem.Road = Block;
			Ctrl.Velocity = Response[Block][0].Speed;
			Elem.State = Response[Block][0].State;
			IMU_SetYaw(Response[Block][0].Angle);
		}
	}
}

// 记录模式相当于对赛道进行微分，失败了因为数值很难调
//  Record_type xdata Record_GarageOut[512];

// void Elem_Record(void)
// {
// 	static fp32 Angle = 0, lastAngle = 0;
// 	static uint16 Count = 0;

// 	if (Elem.Record == ENABLE)
// 	{
// 		// 记录赛道的退出按钮程序
// 		switch (Page.BtnNum)
// 		{
// 		case BUTTON_CLICKED:
// 			break;
// 		case BUTTON_DOUBLE:
// 			break;
// 		case BUTTON_LONG:
// 			Elem.Record = DISABLE;
// 			Enco.Displace = 0;
// 			Count = 0;
// 			break;
// 		}
// 		if (Enco.Displace >= 100)
// 		{
// 			// 想用malloc和链表存储不定长数据的，但stdlib不好使，自己写不现实，上freertos困难
// 			// 所以用结构体数组进行保存，只要定好长度就行
// 			Enco.Displace = 0;
// 			Angle = IMU_CalcYawRef();
// 			IMU_SetYaw(Angle);
// 			Record_GarageOut[Count].Step = Count;
// 			Record_GarageOut[Count].DiffYaw = -(Angle - lastAngle);
// 			// if (M_ABS(Record_GarageOut[Count].DiffYaw) < 0.5f)
// 			// {
// 			// 	Record_GarageOut[Count].DiffYaw = 0;
// 			// }
// 			if (M_ABS(Record_GarageOut[Count].DiffYaw) > 180)
// 			{
// 				Record_GarageOut[Count].DiffYaw = Record_GarageOut[Count - 1].DiffYaw;
// 			}
// 			lastAngle = Angle;
// 			// DMA_printf("%d,%f\n", Record_GarageOut[Count].Step, Record_GarageOut[Count].DiffYaw);
// 			DMA_printf("%f\n", Record_GarageOut[Count].DiffYaw);
// 			Count++;
// 		}
// 	}
// }

// if (Record_GarageOut[Elem.Step].Step == Elem.Step)
// {
// 	if ((Enco.Displace >= 100))
// 	{
// 		i++;
// 		if (i > 30)
// 		{
// 			if ((M_ABS(IMU_CalcYawRef()) < 0.6f))
// 			{
// 				Enco.Displace = 0;
// 				IMU_SetYaw(Record_GarageOut[Elem.Step].DiffYaw);
// 				Elem.Step++;
// 			}
// 		}
// 		else
// 		{
// 			Enco.Displace = 0;
// 			IMU_SetYaw(Record_GarageOut[Elem.Step].DiffYaw);
// 			Elem.Step++;
// 		}
// 	}
// }
// else
// {
// 	Elem.State = QUIT;
// 	Enco.Displace = 0;
// 	Elem.Road = No_Elem;
// 	Elem.Step = 0;
// 	Buzz_OFF();
// }
